/*
 * OREKIT-X
 * Copyright 2002-2008 CS Communication & Systemes
 * 
 * Parts of this software package have been licensed to CS
 * Communication & Systemes (CS) under one or more contributor license
 * agreements.  See the NOTICE file distributed with this work for
 * additional information.
 *  
 * This is an experimental copy of OREKIT from www.orekit.org.
 * Please use the original OREKIT from orekit.org for normal work
 * unrelated to this research project.
 * 
 * Licensed under the Apache License, Version 2.0 (the "License"); you
 * may not use this file except in compliance with the License.  You
 * may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
 * implied.  See the License for the specific language governing
 * permissions and limitations under the License.
 */
package ore.orbits;

import ore.Frame;
import ore.PositionVelocity;
import ore.errors.OrekitException;
import ore.time.AbsoluteDate;

import org.apache.commons.math.geometry.Vector3D;

/**
 * This class handles equinoctial orbital parameters, which can support both
 * circular and equatorial orbits.
 *
 * The parameters used internally are the equinoctial elements which can be
 * related to keplerian elements as follows:
 *   <pre>
 *     a
 *     ex = e cos(&omega; + &Omega;)
 *     ey = e sin(&omega; + &Omega;)
 *     hx = tan(i/2) cos(&Omega;)
 *     hy = tan(i/2) sin(&Omega;)
 *     lv = v + &omega; + &Omega;
 *   </pre>
 * where &omega; stands for the Perigee Argument and &Omega; stands for the
 * Right Ascension of the Ascending Node.
 *
 * The conversion equations from and to keplerian elements given above hold only
 * when both sides are unambiguously defined, i.e. when orbit is neither equatorial
 * nor circular. When orbit is either equatorial or circular, the equinoctial
 * parameters are still unambiguously defined whereas some keplerian elements
 * (more precisely &omega; and &Omega;) become ambiguous. For this reason, equinoctial
 * parameters are the recommended way to represent orbits.
 * 
 * An instance of this class is immutable.
 * 
 * @see    ore.Orbit
 * @see    KeplerianOrbit
 * @see    CircularOrbit
 * @see    CartesianOrbit
 * 
 * @author Mathieu Rom&eacute;ro
 * @author Luc Maisonobe
 * @author Guylaine Prat
 * @author Fabien Maussion
 * @author V&eacute;ronique Pommier-Maurussane
 */
public class EquinoctialOrbit 
    extends ore.Orbit
{
    public static final int MEAN_LATITUDE_ARGUMENT = 0;
    public static final int ECCENTRIC_LATITUDE_ARGUMENT = 1;
    public static final int TRUE_LATITUDE_ARGUMENT = 2;


    /** 
     * Semi-major axis (m).
     */
    public final double a;
    /** 
     * First component of the eccentricity vector.
     */
    public final double ex;
    /** 
     * Second component of the eccentricity vector.
     */
    public final double ey;
    /** 
     * First component of the inclination vector. 
     */
    public final double hx;
    /** 
     * Second component of the inclination vector. 
     */
    public final double hy;
    /** 
     * True latitude argument (rad). 
     */
    public final double lv;


    /** 
     * @param a  semi-major axis (m)
     * @param ex e cos(&omega; + &Omega;), first component of eccentricity vector
     * @param ey e sin(&omega; + &Omega;), second component of eccentricity vector
     * @param hx tan(i/2) cos(&Omega;), first component of inclination vector
     * @param hy tan(i/2) sin(&Omega;), second component of inclination vector
     * @param l  (M or E or v) + &omega; + &Omega;, mean, eccentric or true latitude argument (rad)
     * @param type type of latitude argument, must be one of {@link #MEAN_LATITUDE_ARGUMENT},
     * {@link #ECCENTRIC_LATITUDE_ARGUMENT} or  {@link #TRUE_LATITUDE_ARGUMENT}
     * @param frame the frame in which the parameters are defined
     * @param date date of the orbital parameters
     * @param mu central attraction coefficient (m<sup>3</sup>/s<sup>2</sup>)
     * @exception IllegalArgumentException if the longitude argument type is not
     * one of {@link #MEAN_LATITUDE_ARGUMENT}, {@link #ECCENTRIC_LATITUDE_ARGUMENT}
     * or {@link #TRUE_LATITUDE_ARGUMENT}
     * @see #MEAN_LATITUDE_ARGUMENT
     * @see #ECCENTRIC_LATITUDE_ARGUMENT
     * @see #TRUE_LATITUDE_ARGUMENT
     */
    public EquinoctialOrbit(final double a, final double ex, final double ey,
                            final double hx, final double hy,
                            final double l, final int type,
                            final Frame frame, final AbsoluteDate date, final double mu)
        throws IllegalArgumentException
    {
        super(frame, date, mu);
        this.a  =  a;
        this.ex = ex;
        this.ey = ey;
        this.hx = hx;
        this.hy = hy;

        switch (type) {
        case MEAN_LATITUDE_ARGUMENT :
            this.lv = computeLM(l);
            break;
        case ECCENTRIC_LATITUDE_ARGUMENT :
            this.lv = computeLE(l);
            break;
        case TRUE_LATITUDE_ARGUMENT :
            this.lv = l;
            break;
        default :
            throw OrekitException.createIllegalArgumentException(
                  "angle type not supported, supported angles: {0}, {1} and {2}",
                  "MEAN_LATITUDE_ARGUMENT", "ECCENTRIC_LATITUDE_ARGUMENT",
                  "TRUE_LATITUDE_ARGUMENT");
        }
    }
    /** 
     * Cartesian parameters.
     * @param pvCoordinates the position end velocity
     * @param frame the frame in which are defined the {@link PositionVelocity}
     * @param date date of the orbital parameters
     * @param mu central attraction coefficient (m<sup>3</sup>/s<sup>2</sup>)
     */
    public EquinoctialOrbit(final PositionVelocity pvCoordinates, final Frame frame,
                            final AbsoluteDate date, final double mu)
    {
        super(pvCoordinates, frame, date, mu);
        /*
         * Compute semi-major axis
         */
        Vector3D pvP = pvCoordinates.getPosition();
        Vector3D pvV = pvCoordinates.getVelocity();
        double r = pvP.getNorm();
        double V2 = Vector3D.dotProduct(pvV, pvV);
        double rV2OnMu = r * V2 / mu;
        this.a = r / (2 - rV2OnMu);
        /*
         * Compute inclination vector
         */
        Vector3D w = Vector3D.crossProduct(pvP, pvV).normalize();
        double d = 1.0 / (1 + w.getZ());
        this.hx = -d * w.getY();
        this.hy =  d * w.getX();
        /*
         * Compute true latitude argument
         */
        Vector3D p = pvP;
        double cLv = (p.getX() - d * p.getZ() * w.getX()) / r;
        double sLv = (p.getY() - d * p.getZ() * w.getY()) / r;
        this.lv = Math.atan2(sLv, cLv);
        /*
         * Compute eccentricity vector
         */
        double eSE = Vector3D.dotProduct(p, pvV) / Math.sqrt(mu * a);
        double eCE = rV2OnMu - 1;
        double e2  = eCE * eCE + eSE * eSE;
        double f   = eCE - e2;
        double g   = Math.sqrt(1 - e2) * eSE;
        this.ex = this.a * (f * cLv + g * sLv) / r;
        this.ey = this.a * (f * sLv - g * cLv) / r;
    }
    /** 
     * @param op Orbital parameters to copy
     */
    public EquinoctialOrbit(ore.Orbit op) {
        super(op);
        this.a  = op.getA();
        this.ex = op.getEquinoctialEx();
        this.ey = op.getEquinoctialEy();
        this.hx = op.getHx();
        this.hy = op.getHy();
        this.lv = op.getLv();
    }


    /** 
     * @return semi-major axis (m)
     */
    public double getA() {
        return this.a;
    }
    /** 
     * @return e cos(&omega; + &Omega;), first component of the eccentricity vector
     */
    public double getEquinoctialEx() {
        return this.ex;
    }
    /** 
     * @return e sin(&omega; + &Omega;), second component of the eccentricity vector
     */
    public double getEquinoctialEy() {
        return this.ey;
    }
    /** 
     * @return tan(i/2) cos(&Omega;), first component of the inclination vector
     */
    public double getHx() {
        return this.hx;
    }
    /** 
     * @return tan(i/2) sin(&Omega;), second component of the inclination vector
     */
    public double getHy() {
        return this.hy;
    }
    /** 
     * @return eccentricity
     */
    public double getE() {
        return Math.sqrt((this.ex * this.ex) +
                         (this.ey * this.ey));
    }
    /** 
     * @return inclination (rad)
     */
    public double getI() {
        return (2.0 * Math.atan(Math.sqrt(hx * hx + hy * hy)));
    }
    /** 
     * @return v + &omega; + &Omega; true latitude argument (rad)
     */
    public double getLv() {
        return this.lv;
    }
    /** 
     * @return E + &omega; + &Omega; eccentric latitude argument (rad)
     */
    public double getLE() {
        double epsilon = Math.sqrt(1 - (this.ex * this.ex) - (this.ey * this.ey));
        double cosLv   = Math.cos(this.lv);
        double sinLv   = Math.sin(this.lv);
        double num     = (this.ey * cosLv) - (this.ex * sinLv);
        double den     = epsilon + 1 + (this.ex * cosLv) + (this.ey * sinLv);
        return (this.lv + 2.0 * Math.atan(num / den));
    }
    /** 
     * @return M + &omega; + &Omega; mean latitude argument (rad)
     */
    public double getLM() {
        double lE = this.getLE();
        return (lE - (this.ex * Math.sin(lE)) + (this.ey * Math.cos(lE)));
    }
    /** 
     * @param lE = E + &omega; + &Omega; eccentric latitude argument (rad)
     * @return the true latitude argument
     */
    private double computeLE(double lE) {
        double epsilon = Math.sqrt(1 - (this.ex * this.ex) - (this.ey * this.ey));
        double cosLE   = Math.cos(lE);
        double sinLE   = Math.sin(lE);
        double num = (this.ex * sinLE) - (this.ey * cosLE);
        double den = epsilon + 1 - (this.ex * cosLE) - (this.ey * sinLE);
        return (lE + 2.0 * Math.atan(num / den));
    }
    /** 
     * @param lM = M + &omega; + &Omega; mean latitude argument (rad)
     * @return the true latitude argument
     */
    private double computeLM(double lM) {
        /*
         * Generalization of Kepler equation to equinoctial parameters
         * with lE = PA + RAAN + E and
         *      lM = PA + RAAN + M = lE - ex.sin(lE) + ey.cos(lE)
         */
        double lE = lM;
        double shift = 0.0;
        double lEmlM = 0.0;
        double cosLE = Math.cos(lE);
        double sinLE = Math.sin(lE);
        int iter = 0;
        do {
            double f2 = (this.ex * sinLE) - (this.ey * cosLE);
            double f1 = 1.0 - (this.ex * cosLE) - (this.ey * sinLE);
            double f0 = lEmlM - f2;

            double f12 = 2.0 * f1;
            shift = (f0 * f12) / (f1 * f12 - f0 * f2);

            lEmlM -= shift;
            lE     = (lM + lEmlM);
            cosLE  = Math.cos(lE);
            sinLE  = Math.sin(lE);

        }
        while ((++iter < 50) && (Math.abs(shift) > 1.0e-12));

        return this.computeLE(lE);
    }

    /**  Returns a string representation of this equinoctial parameters object.
     * @return a string representation of this object
     */
    public String toString() {
        return new StringBuffer().append("equinoctial parameters: ").append('{').
                                  append("a: ").append(a).
                                  append("; ex: ").append(ex).append("; ey: ").append(ey).
                                  append("; hx: ").append(hx).append("; hy: ").append(hy).
                                  append("; lv: ").append(Math.toDegrees(lv)).
                                  append(";}").toString();
    }

}
